#include <iostream>
#include <pcl/common/common_headers.h>
#include <pcl/io/pcd_io.h>
#include <pcl/point_types.h>
#include <pcl/visualization/pcl_visualizer.h>
#include <pcl/visualization/cloud_viewer.h>
#include <pcl/console/parse.h>
#include <pcl/filters/voxel_grid.h>
#include <pcl/filters/approximate_voxel_grid.h>
#include <pcl/ModelCoefficients.h>
#include <pcl/sample_consensus/method_types.h>
#include <pcl/sample_consensus/model_types.h>
#include <pcl/segmentation/sac_segmentation.h>
#include <pcl/sample_consensus/sac_model_cylinder.h>
#include <pcl/features/normal_3d.h>






int main()
{
    pcl::PCDReader reader;
    pcl::VoxelGrid<pcl::PointXYZ> sor;
    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);
    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_filtered(new pcl::PointCloud<pcl::PointXYZ>);
    pcl::ModelCoefficients::Ptr coefficients (new pcl::ModelCoefficients);
	pcl::PointIndices::Ptr inliers (new pcl::PointIndices);
    reader.read<pcl::PointXYZ>("../cylinder.pcd", *cloud);
    std::cerr << "Point cloud data: " << cloud->points.size () << " points" << std::endl;
    pcl::visualization::CloudViewer viewer("xxx");
#if 0
    viewer.showCloud(cloud);
    while (!viewer.wasStopped()){ };
#else
    sor.setInputCloud (cloud);
    sor.setLeafSize (0.01f, 0.01f, 0.01f);
    sor.filter (*cloud_filtered);
    pcl::visualization::CloudViewer viewer("xxx");
    viewer.showCloud(cloud_filtered);
    while (!viewer.wasStopped()){ };
#endif

    std::cerr << "Filtered_Point cloud data: " << cloud_filtered->points.size () << " points" << std::endl;
    /*
	//--------------------------------计算法线-----------------------------------
	pcl::PointCloud<pcl::Normal>::Ptr cloud_normals(new pcl::PointCloud<pcl::Normal>);
	pcl::search::KdTree<pcl::PointXYZ >::Ptr tree(new pcl::search::KdTree<pcl::PointXYZ >);
	pcl::NormalEstimation<pcl::PointXYZ, pcl::Normal> normal_est;
	normal_est.setSearchMethod(tree);
	normal_est.setInputCloud(cloud_filtered);
	normal_est.setKSearch(50);
	normal_est.compute(*cloud_normals);
    */
    // 创建分割对象
    pcl::SACSegmentation<pcl::PointXYZ> seg;
	//pcl::SACSegmentationFromNormals<pcl::PointXYZ, pcl::Normal> seg;
    // 可选设置
    seg.setOptimizeCoefficients (true);
    // 必要的设置
    std::cerr << "init"<< std::endl;
    seg.setNumberOfThreads(1);	
    seg.setModelType(pcl::SACMODEL_CYLINDER);
	//设置使用那个随机参数估计方法为随机样本共识
	seg.setMethodType(pcl::SAC_RANSAC);
    //设置表面法线权重系数
	//seg.setNormalDistanceWeight(0.1);         
	//设置最大迭代数
	seg.setMaxIterations(1000);
	//设置是否为模型内点的距离阈值
	seg.setDistanceThreshold(0.01);
	//设置估计出的圆柱模型的半径的范围
	seg.setRadiusLimits(0.1, 10);

    // 创建模型系数对象，存储结果

    // 调用分割函数来执行
    seg.setInputCloud (cloud_filtered);
    std::cerr << "segment"<< std::endl;
	//seg.setInputNormals(cloud_normals);
    seg.segment (*inliers, *coefficients);

    if (inliers->indices.size () == 0)
    {
        //PCL_ERROR ("Could not estimate a planar model for the given dataset.");
        return (-1);
    }

    std::cerr << "Model coefficients: " << coefficients->values[0] << " " 
                                          << coefficients->values[1] << " "
                                          << coefficients->values[2] << " " 
                                          << coefficients->values[3] << std::endl;

    



    return 0;
}